/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
# example client for position server with swig wrapped c++ library in python
import sys, time
sys.path.append("../../../interfaces/wrap/Python/lib")
sys.path.append("../../../../GIMI/wrap/Python/lib")
import PositionClient as PC
import gimi
if sys.argv.__len__() != 3:
	print "Usage: python %s clientName serverName"%sys.argv[0]
	sys.exit()
g = gimi.GIMI()
pc = PC.CPositionClient(g, 0)
print "connecting... ",
ret = g.connectToHubEx("robogw.dyndns.tv", 40002, sys.argv[1])
print PC.GetErrorStr(ret)
print "set target"
if sys.argv[2].find(".") < 0:
	print sys.argv[2]
	pc.SetDefaultTarget(sys.argv[2])
else:
	print "with fancy name ", sys.argv[2]
	pc.SetDefaultTarget(PC.SMaCISL(sys.argv[2]))
print "open... ",
ret = pc.Open()
print PC.GetErrorStr(ret)
del ret
time.sleep(1.0) # got to sleep to wait for something to get ready to serve us
print "Getting position data"
poseData = PC.CPositionData()
ret = pc.GetPositionEvent(poseData, None, 1000)
if ret:
	print "got some data"
	pose = poseData.GetPose2D()
	tStamp = poseData.GetTimestamp()
	speed = poseData.GetSpeed2D()
	variance = poseData.GetVariance2D()
	if pose:
		print "%s in (%f,%f,%f) "%(sys.argv[2], pose.x, pose.y, pose.a),
		print "at %i.%.6is"%(tStamp.timestamp_s, tStamp.timestamp_us)
		print "with variance of [%f,%f,%f]"%(variance.x, variance.y, variance.a), 
		print " and speed of %f m/s and turning at %f rad/s"%(speed.speed, speed.angularSpeed)
	else:
		print "data was rotten"
else:
	print "Failed to get data"
	
pc.Close()

